#ifndef ROSINTERFACE_H
#define ROSINTERFACE_H

#include <ros/ros.h>
#include <tf/transform_listener.h>

#include <srs_grasping/GetGraspConfigurations.h>
#include <srs_grasping/GetGraspsFromPosition.h>
#include <srs_msgs/GraspSubConfiguration.h>
#include <cob_object_detection_msgs/DetectionArray.h>
#include <geometry_msgs/Pose.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <sensor_msgs/JointState.h>
#include <cob_object_detection_msgs/DetectObjects.h>
#include <kinematics_msgs/GetPositionIK.h>

#define PROCESS_OK				 0
#define PROCESS_ERROR_FORK		-1
#define PROCESS_ERROR_EXEC		-2
#define PROCESS_ERROR_WAITING	-3
#define PROCESS_RUNNING			-4
#define PROCESS_ERRROR_KILL		-5


class RosInterface
{
    public:
        typedef struct joint_values
        {
            std::string name;
            float position;
            float velocity;
            float effort;
        };

        RosInterface();
        virtual ~RosInterface();

        cob_object_detection_msgs::DetectionArray Detect();
        std::vector<srs_msgs::GraspConfiguration> getGraspConfigurations(int object_id);
        std::vector<srs_msgs::GraspSubConfiguration> getGraspsFromPosition(int object_id, geometry_msgs::Pose object_pose);
        void move(std::string component,std::vector<float> values);
        int image_view(char *topic);
        int run_OpenRave(int object_id);
        int run_rviz();
        void joint_status();
        void callback(const sensor_msgs::JointState::ConstPtr &msg);
        void simulate_grasp(int object_id, srs_msgs::GraspSubConfiguration grasp, geometry_msgs::Pose object_pose);
        joint_values * getJointValues(std::string component);
        std::string toString(RosInterface::joint_values jv);
        std::string toStringAll(RosInterface::joint_values *jv);
        geometry_msgs::Pose transformPose(geometry_msgs::PoseStamped p);
        std::vector<float> callIkSolver(std::vector<float> current_joint_configuration, geometry_msgs::PoseStamped target_pose);
        bool computeIK(std::vector<float> jnt_pos, const geometry_msgs::PoseStamped &pose, std::vector<float> &solution);

    protected:

        ros::NodeHandle n;
        ros::ServiceClient detection_client;
        ros::ServiceClient getgraspconfigurations_client;
        ros::ServiceClient getgraspsfromposition_client;
        ros::ServiceClient simulategrasp_client;
        ros::ServiceClient ik_client;

        ros::Subscriber sub;
        //ros::Subscriber sub_sdh_controller;    // Suscriber
        //ros::Publisher pub_sdh_controller;     // publisher

    private:
    	pid_t pid_OR;
    	pid_t pid_RVIZ;
    	pid_t pid_IMAGE_VIEW;

        joint_values *joints;
        joint_values arm_joints[7];
        joint_values sdh_joints[7];
        joint_values torso_joints[3];
        joint_values tray_joints[1];
        joint_values head_joints[1];
};

#endif // ROSINTERFACE_H
